﻿#include "tm_robotitem.h"

#include <QPainter>
#include "math.h"
#include <QDebug>

TM_RobotItem::TM_RobotItem(qreal w,qreal h,qreal radius,QGraphicsItem *parent):
    QGraphicsItem(parent)
{
    if(radius > 0)
    {
        m_X = -radius;
        m_Y = -radius;
        m_W = radius*2.0;
        m_H = radius*2.0;
        m_radius = radius;
    }
    else
    {
        m_W = w;
        m_H = h;
        m_X = -m_W/2;
        m_Y = -m_H/2;
        m_radius = 0;
    }

    this->setFlags(QGraphicsItem::ItemIsSelectable);
    this->setZValue(3);
    this->setData(ItemId,"TM_Navi");
    this->setData(ItemDesciption,"TM_Navi");
}

void TM_RobotItem::on_Set_RadirData(QString type,QStringList dataList)
{
    if(type == g_RobotScan_multi)
        m_RadirData = dataList;
}

QRectF TM_RobotItem::boundingRect() const
{
    return QRectF(m_X,m_Y,m_W,m_H);
}

void TM_RobotItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
{
    painter->setPen(QPen(QColor(0,255,0,150),0));
    painter->setBrush(QBrush(QColor(0,255,0,150)));
    if(m_radius > 0)
        painter->drawEllipse(m_X,m_Y,m_W,m_H);
    else
        painter->drawRect(m_X,m_Y,m_W,m_H);
    painter->setBrush(QBrush(QColor(255,0,0,200)));
    QPoint point[3] = {QPoint(m_W/2,0),QPoint(m_W/2-(m_W/4),-(m_W/4)),QPoint(m_W/2-(m_W/4),(m_W/4))};
    painter->drawConvexPolygon(point,3);
    painter->setPen(QPen(Qt::red,2));

    //尾 +-180，头：0 上：负 下：正
    for(qreal i=0;i<m_RadirData.length();i++)
    {
        // double jiaodu = intensitiesList[i].toDouble()+3.1415926;
        // double index =  jiaodu*180.0/3.1415926;
        // qDebug()<<"ssssssssssss"<<QString::number(index);
        // if(index >=0 && index <= 360)
        // {
        //     EndDataList[index] = rangesList[i].toString();
        // }
        //qDebug()<<"EndPoint="<<jiaodu<<len<<hudu<<cos(hudu)<<sin(hudu)<<X<<Y<<g_map_resolution<<qreal(X/g_map_resolution)<<qreal(Y/g_map_resolution);
        if("/scan_multi2" == g_RobotScan_multi)
        {
            QStringList RadirDataList = QString(m_RadirData[i]).split("=",Qt::SkipEmptyParts);
            //qDebug()<<"sssssssssssss7984326"<<QString(m_RadirData[i])<<RadirDataList;
            if(RadirDataList.length() < 2)
                continue;
            qreal hudu = -QString(RadirDataList[0]).toDouble();//点云方向和工具方向相反，取负
            qreal len = QString(RadirDataList[1]).toDouble();
            qreal X = (len * cos(hudu));
            qreal Y = (len * sin(hudu));

            qreal dX = qreal(X/g_map_resolution)-m_x_shift;//点云数据是相对于地图整体的，取出偏移量
            qreal dY = qreal(Y/g_map_resolution)+m_y_shift;//点云数据是相对于地图整体的，取出偏移量
            qreal dbNewX = dX * cos(-this->rotation() * 3.1415926 / 180.0) - dY * sin(-this->rotation() * 3.1415926 / 180.0);
            qreal dbNewY = dX * sin(-this->rotation() * 3.1415926 / 180.0) + dY * cos(-this->rotation() * 3.1415926 / 180.0);

            painter->drawPoint(dbNewX,dbNewY);
        }
        if("/scan_multi" == g_RobotScan_multi)
        {
            if(QString(m_RadirData[i]).length() < 1)
                continue;
            qreal jiaodu = m_radirStartJiaoDu-i;
            //x=r*cos角 y=r*sin角
            qreal len = QString(m_RadirData[i]).toDouble();

            qreal hudu = jiaodu/180.0*3.1416;
            qreal X = len * cos(hudu);
            qreal Y = len * sin(hudu);
            painter->drawPoint(qreal(X/g_map_resolution),qreal(Y/g_map_resolution));
        }
    }
}
